#pragma once
#include <highgui.h>

namespace testSet
{
	const float znear = 550;
	const float zfar = znear + 1000;

	class testUnit
	{
	protected:
		float _camera[9];
		float _d[4];

	public:
		char filePath[255];
		float m[16];
		CvMat Camera;
		CvMat Distortion;
		
		testUnit(char* imagePath, float camera[9], float r[3], float t[3],float d[4])
		{
			memcpy(_camera, camera, 9*sizeof(float));
			Camera = cvMat(3,3, CV_32F, _camera);

			memcpy(_d, d, 4*sizeof(float));
			Distortion = cvMat(1, 4, CV_32F, _d);

			strcpy(filePath, imagePath);
			SetRT(r,t);
		}

		void SetRT(float r[3], float t[3])
		{
			CvMat rotation = cvMat( 1, 3, CV_32F, r);
			CvMat* R = cvCreateMat(3,3, CV_32F);
			cvRodrigues2(&rotation, R);
			
			m[0] = R->data.fl[0];
			m[1] = R->data.fl[3];
			m[2] = R->data.fl[6];
			m[3] = 0;

			m[4] = R->data.fl[1];
			m[5] = R->data.fl[4];
			m[6] = R->data.fl[7];
			m[7] = 0;

			m[8] = R->data.fl[2];
			m[9] = R->data.fl[5];
			m[10] =R->data.fl[8];
			m[11] = 0;

			m[12] = t[0];
			m[13] = t[1];
			m[14] = t[2];
			m[15] = 1;

			cvReleaseMat(&R);

		}

		void setupFrustum(float w, float h)
		{
			float cx = _camera[2];
			float cy = _camera[5];
			float fx = _camera[0];
			float fy = _camera[4];

			glFrustum( -znear*cx / fx, znear*(w - cx) / fx,
					   -znear*cy / fy, znear*(h - cy) / fy, 
			znear, zfar);			
		}	
	};

	const int testCount = 2;
	testUnit* tests[testCount];

	void Init()
	{
		float c1[] = {5.4984992042289991e+002, 0., 3.0645354074900519e+002, 0., 5.3505879399060700e+002, 2.5459202380218574e+002, 0., 0., 1.};
		float r1[] = {1.94344163e+000, 2.19139075e+000, 6.48129046e-001};
		float t1[] = {-1.86349416e+000, -3.66504836e+000, 1.12549124e+001};
		float d1[] = {-2.6830884884127926e-001, -4.3756678937236455e-002, -8.8667295216177840e-003, 9.1628041709185520e-003 };
		tests[0] = new testUnit("C:\\Inco\\OpenCV2.1\\samples\\c\\left09.jpg", c1,r1,t1, d1);

		float c2[] = {5.7291609867013528e+002, 0., 3.2610417927354649e+002, 0., 5.8173582743077156e+002, 2.6320080617076576e+002, 0., 0., 1.};
		float r2[] = { -2.35292006e+000, -1.93340540e-001, 2.16562986e-001};
		float t2[] = {-1.83015060e+000, 1.80811465e+000, 1.44920006e+001};
		float d2[] = {-3.4904083933143709e-001, 1.2404147547446789e-001, -1.3594438773272233e-002, 7.8754140384982539e-003 };
		tests[1] = new testUnit("C:\\Inco\\OpenCV2.1\\samples\\c\\left02.jpg", c2,r2,t2,d2);


		// next 2 test are inerestiong (this grabbed from my own new web-camera) 
		// and i wont compare visual results of reconstrution of single image, BUT from different calibration data 
		// first single saple calibration
		float c3[] = { 4.6421034070915749e+002, 0., 2.7175154957496341e+002, 0., 4.5705277273972757e+002, 2.3527912395983134e+002, 0., 0., 1.};
		float r3[] = { 2.13486099e+000, 1.73877692e+000, 8.20665509e-002};
		float t3[] = {-2.84254730e-001, 5.55957973e-001, 1.57344027e+001};
		float d3[] = { -1.5827829606651195e-001, 2.8747519776864011e-001, 1.4894402459581452e-003, -5.7332262110187124e-004 };
		tests[2] = new testUnit("snapshots\\0.png", c3,r3,t3, d3);

		// second 8 generalization calibration
		float c4[] = {5.7291609867013528e+002, 0., 3.2610417927354649e+002, 0., 5.8173582743077156e+002, 2.6320080617076576e+002, 0., 0., 1.};
		float r4[] = {2.11474323e+000, 1.68571603e+000, 1.52433768e-001};
		float t4[] = { -1.88805461e+000, 5.25527000e-002, 1.90509434e+001};
		float d4[] = { -3.6925804911883610e-002, 9.5458576728206408e-003, 4.7933795410162105e-003, -1.4066602837854646e-002 };
		tests[3] = new testUnit("snapshots\\0.png", c4,r4,t4,d4);

		// not worked test
		//float c5[] = {5.5260131842738724e+002, 0., 3.1782619751044928e+002, 0., 5.6074491388546733e+002, 3.0641682841662453e+002, 0., 0., 1.};
		float c5[] = {5.7291609867013528e+002, 0., 3.2610417927354649e+002, 0., 5.8173582743077156e+002, 2.6320080617076576e+002, 0., 0., 1.}; // G
		float r5[] = { 3.16927791e-001, 2.44248414e+000, 3.23947340e-001};
		float t5[] = { 6.05233192e-001, -5.48697376e+000, 1.80900497e+001};
		//float d5[] = { 1.3205181770460578e-001, -1.1852374938230332e+000, 3.8696281229193101e-002, -1.6806482152124404e-002 };
		float d5[] = {  -3.6925804911883610e-002, 9.5458576728206408e-003, 4.7933795410162105e-003, -1.4066602837854646e-002};  //G
		tests[4] = new testUnit("snapshots\\3.png", c5,r5,t5,d5);

	}

}